package roboProject.steering;

import java.util.Stack;

import lejos.robotics.navigation.SimpleNavigator;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.proposal.WayPoint;

public class RoboPilot extends SimpleNavigator {
	
	private TachoPilot tachoPilot;
	private float _wheelDiameter = 5.7f;
	private Stack<WayPoint> moveHistory = new Stack<WayPoint>();
	
	public RoboPilot(TachoPilot pilot) {
		super(pilot);
		tachoPilot = pilot;
		tachoPilot.setMoveSpeed(720);
		tachoPilot.setTurnSpeed(90);
	}
	
	public void floatMotors(){
		tachoPilot.getLeft().flt();
		tachoPilot.getRight().flt();
	}
	
	public void goToHome(){
		WayPoint w = null;
		while(!moveHistory.isEmpty()){
			w = moveHistory.pop();
			goTo((float)w.getX(), (float)w.getY());
		}
		goTo(0, 0);
	}
	
	//TODO:backwards & steering is shit... actually with that release of leJOS
	public void turnLeft(){
		if(!isMoving()){
			moveHistory.push(new WayPoint(this.getX(), this.getY()));
			rotate(360f, true);
		}
	}
	
	public void turnRight(){
		if(!isMoving()){
			moveHistory.push(new WayPoint(this.getX(), this.getY()));
			rotate(-360f, true);
		}
	}
		
	public int getSpeedInMilimeterPerSecond(){
		return (int)(((_wheelDiameter * 3.1415) / 360) * tachoPilot.getRight().getRotationSpeed())*10;		
	}
}
